# Modern Robotics Descriptions for all various Interbotix Universal Factory Arms.
# Note that the end-effector is positioned at '<robot_name>/linkX' where 'X' is
# a number from 5-7 corresponding to the last link on the robot.
# The Space frame is positioned at '<robot_name>/base_link'.

import numpy as np

class uxarm5:
    Slist = np.array([[0.0, 0.0, 1.0, 0.0, 0.0, 0.0],
                      [0.0, 1.0, 0.0, -0.267, 0.0, 0.0],
                      [0.0, 1.0, 0.0, -0.5515, 0.0, 0.0535],
                      [0.0, 1.0, 0.0, -0.209, 0.0, 0.131],
                      [0.0, 0.0, -1.0, 0.0, 0.207, 0.0]]).T

    M = np.array([[1.0, 0.0, 0.0, 0.207],
                  [0.0, -1.0, 0.0, 0.0],
                  [0.0, 0.0, -1.0, 0.112],
                  [0.0, 0.0, 0.0, 1.0]])

    Guesses = [[0, 0, -1.5708, 0, 0],
               [2.0944, 0, -1.5708, 0, 0],
               [-2.0944, 0, -1.5708, 0, 0]]

class uxarm6:
    Slist = np.array([[0.0, 0.0, 1.0, 0.0, 0.0, 0.0],
                      [0.0, 1.0, 0.0, -0.267, 0.0, 0.0],
                      [0.0, 1.0, 0.0, -0.5515, 0.0, 0.0535],
                      [0.0, 0.0, -1.0, 0.0, 0.131, 0],
                      [0.0, 1.0, 0.0, -0.209, 0.0, 0.131],
                      [0.0, 0.0, -1.0, 0.0, 0.207, 0.0]]).T

    M = np.array([[1.0, 0.0, 0.0, 0.207],
                  [0.0, -1.0, 0.0, 0.0],
                  [0.0, 0.0, -1.0, 0.112],
                  [0.0, 0.0, 0.0, 1.0]])

    Guesses = [[0, 0, -1.5708, 0, 0, 0],
               [2.0944, 0, -1.5708, 0, 0, 0],
               [-2.0944, 0, -1.5708, 0, 0, 0]]

class uxarm7:
    Slist = np.array([[0.0, 0.0, 1.0, 0.0, 0.0, 0.0],
                      [0.0, 1.0, 0.0, -0.267, 0.0, 0.0],
                      [0.0, 0.0, 1.0, 0.0, 0.0, 0.0],
                      [0.0, -1.0, 0.0, 0.5515, 0.0, -0.0535],
                      [0.0, 0.0, -1.0, 0.0, 0.131, 0],
                      [0.0, 1.0, 0.0, -0.209, 0.0, 0.131],
                      [0.0, 0.0, -1.0, 0.0, 0.207, 0.0]]).T

    M = np.array([[1.0, 0.0, 0.0, 0.207],
                  [0.0, -1.0, 0.0, 0.0],
                  [0.0, 0.0, -1.0, 0.112],
                  [0.0, 0.0, 0.0, 1.0]])

    Guesses = [[0, 0, 0, 1.5708, 0, 0, 0],
               [2.0944, 0, 0, 1.5708, 0, 0, 0],
               [-2.0944, 0, 0, 1.5708, 0, 0, 0]]
